% 多飞行器协同末制导任务
% Td为预设值

clc
clear
clear global
global hInt hGui N1 N2 ku kv kr kdelta Td T0 d2r mass g S 
global alphamin alphamax alphaRateLimit bankRateLimit
hInt = 0.01; % 积分步长
hGui = 0.1; % 制导周期
hk = hGui/hInt;
d2r = pi/180;
N1 = 3;
N2 = 3;
ku = 10;
kv = 4;
kr = 1e-6;
kdelta = 8;
g = 9.81;
alphaRateLimit = 5*pi/180;
bankRateLimit = 40*pi/180;
alphamin = -5*pi/180;
alphamax = 25*pi/180;
mass = 1600;
S = 0.5026;
% Td = round(max([T01, T02, T03]) + 4);
Td = 44;

state01 = [30e3 -30e3  20e3 1600 2*d2r -60*d2r];
state02 = [25e3  30e3  22e3 1600 2*d2r -130*d2r];
state03 = [-25e3 -30e3  20e3 1600 5*d2r 0*d2r];
% %% PNG
num = 6e3;
res = nan(num,17);% res: t x y z V fpa azi lambda phi tPNG alphad bankd et r dr delta eta
res(1, 1:7) = [0 state01];
for i = 2:num 
    res_past = res(i-1,:);
    res_new = PNGguidanceloop(res_past);
    res(i,:) = res_new;
    if res(i, 4) <= 0
        num = i-1;
        break;
    end
end
T01 = num*hInt;

num = 6e3;
res = nan(num,17);
res(1, 1:7) = [0 state02];
for i = 2:num 
    res_past = res(i-1,:);
    res_new = PNGguidanceloop(res_past);
    res(i,:) = res_new;
    if res(i, 4) <= 0
        num = i-1;
        break;
    end
end
T02 = num*hInt;

num = 6e3;
res = nan(num,17);
res(1, 1:7) = [0 state03];
for i = 2:num 
    res_past = res(i-1,:);
    res_new = PNGguidanceloop(res_past);
    res(i,:) = res_new;
    if res(i, 4) <= 0
        num = i-1;
        break;
    end
end
T03 = num*hInt;

%% ITCG1
T0 = T01;
hkv = 10;
num = (Td + 1)/hInt;
res = nan(num,17);
res(1, 1:7) = [0 state01];
res(1, 10) = T01;
for i = 2:num 
    res_past = res(i-1,:);
    if hkv >= hk
        res_new = ITCGguidanceloop(res_past);
        hkv = 0;
    else
        res_new = ITCGloop(res_past);
    end
    res(i,:) = res_new;
    hkv = hkv + 1;
    if res(i, 4) <= 0
        num = i-1;
        break;
    end
end
ITCG1 = res(1:num, :);
save('data.mat', 'ITCG1', '-append');
%% ITCG2
T0 = T02;
num = (Td+1)/hInt;
hkv = 10;
res = nan(num,17);
res(1, 1:7) = [0 state02];
res(1, 10) = T02;
for i = 2:num 
    res_past = res(i-1,:);
    if hkv >= hk
        res(i,:) = ITCGguidanceloop(res_past);
        hkv = 0;
    else
        res(i,:) = ITCGloop(res_past);
    end
    hkv = hkv + 1;
    if res(i, 4) <= 0
        num = i-1;
        break;
    end
end
ITCG2 = res(1:num, :);
save('data.mat', 'ITCG2', '-append');
%% ITCG3
T0 = T03;
num = (Td+1)/hInt;
hkv = 10;
res = nan(num,17);
res(1, 1:7) = [0 state03];
res(1, 10) = T03;
for i = 2:num 
    res_past = res(i-1,:);
    if hkv >= hk
        res(i,:) = ITCGguidanceloop(res_past);
        hkv = 0;
    else
        res(i,:) = ITCGloop(res_past);
    end
    hkv = hkv + 1;
    if res(i, 4) <= 0
        num = i-1;
        break;
    end
end
ITCG3 = res(1:num, :);
save('data.mat', 'ITCG3', '-append');